#include "common.h"

#define AUTON_DIST	50

char MoveRobot(void);
//typedef struct { long left, right; } Point;

/***********************************************************************************/
struct autonomous {
	char returnValue;
	char taskPhase;
	char stallCount;	//number of times robot has stalled in routine

	unsigned alreadyRan:1;
//	unsigned dontLaunchBalls:1;	//saved from OI when disabled
	
	unsigned char distance;		//saved to eeprom
	
		
}	pAutonomous={kResultNotRunning,0,0,0,0};


/*******************************************************************/
//called to allow multiple autonomous runs
void AutonomousReset(void)
{
	pAutonomous.alreadyRan = 0;
	pAutonomous.returnValue=kResultNotRunning;
}
/*******************************************************************/
char AutonomousStatus(void)
{
	return pAutonomous.returnValue;	
}
/*******************************************************************/
void AutonomousAbort(void)
{
	pAutonomous.returnValue=kResultNotRunning;	
	pAutonomous.alreadyRan = 1;	
}

/*******************************************************************/
void AutonomousInitialize(void)
{
	if (pAutonomous.returnValue==kResultRunning)
		return;	//do nothing
	if (pAutonomous.alreadyRan)	//must be cleared before running again.
	{
		pAutonomous.returnValue = kResultError;
		return;
	}
	pAutonomous.returnValue=kResultRunning;	//prevent running twice
	pAutonomous.stallCount=0;	//prevent running twice
	pAutonomous.taskPhase=0;
	pAutonomous.distance = AUTON_DIST;
}


/***********************************************************************************/
char AutonomousRun(void)
{
	enum { kHaltBeforePhase = 100 };	//for debugging; stop at phase #
	overlay char advancePhase;	//when set true, state machine will advance to next state
	overlay char result;
	overlay int turnETicks;	//turn measured in encoder ticks (R-L) so that positive = CCW (left turn)
	overlay int distance;
	overlay int timer;
	static char stallReturnPhase = -1;
	static unsigned char wait = 0;	//wait this number of cycles; max 255, or about 6 sec
	
	char CameraTrackingState;

	if (gLoop.onSecondA)
		printf("Autonomous phase: %d\r", (int)pAutonomous.taskPhase);
	
	if (!autonomous_mode)
	{
		AutonomousAbort();	//added to avoid loops in autonomous
		printf("left autonomous mode early\r");
	}

	CameraTrackingState = GetRawTrackingState();

	do {
		result = advancePhase=0;	//clear advancePhase command and result

		if (wait)
		{
			wait--;
			printf("waiting...\r");
			continue;	//skip loop	(advancePhase is 0, so will come back next cycle)
		}
		switch (pAutonomous.taskPhase) {
			case 0:		//initialization; set first destination
				printf("\Starting autonomous mode. ");
				PrintTime();			
				advancePhase=1;
				break;
			case 1:

				wait=0;
				break;
			case 2:
				timer = (1200)/26.2;	//set timer for drive motors (too far -- up ramp)
				timer = (int) pAutonomous.distance*100/262;
				
				printf("Starting move for %d ticks\r", (int)timer);
				advancePhase=1;
				break;
			case 3:
				if (timer>0)
				{
					char speed=(0.90*127);
					--timer;
					gMotorSpeed.cimL = gMotorSpeed.cimR = speed;
//					mPWMLeftCIM = removeESCDeadband(-speed);	//handles limiting too
//					mPWMRightCIM = removeESCDeadband(speed);
				}
				else
					advancePhase=1;
				break;
			case 4:
				//idle here
				
				break;

			default:
				if (pAutonomous.returnValue == kResultRunning)
					pAutonomous.returnValue = kResultNotRunning;
				break;	//quit;
		}	//end of switch


		if (result==kResultTimeOut)
		{	
			pAutonomous.returnValue = kResultTimeOut;
			printf("Timeout\r");
		}
		if (advancePhase)	//diagnostics
		{
			printf("\rEnd of AutoPhase=%d\r",(int)pAutonomous.taskPhase);
			PrintTime();
//			PrintDistanceFeet();
//			PrintVelocity();
		}
		if (advancePhase)
		{
			if (pAutonomous.taskPhase >= kHaltBeforePhase)	//For testing
			{
				pAutonomous.returnValue = kResultNotRunning;
				break;	//exit the main loop
			}

			pAutonomous.taskPhase++;
		}
	} while (advancePhase);
	
	if (kResultRunning != pAutonomous.returnValue)
		pAutonomous.alreadyRan = 1;

	return pAutonomous.returnValue;
}
/*****************************************************************************************/

void ReadAutonomousDistanceEPROM(void)
{
	pAutonomous.distance = EEPROM_read( kEPROMAdr_Autonomous );
}
//********************************************************************************

//#if 1	//select method 1 or 2
/*void SetAutonomousDistance(void)
{
//	static char dirty=0;

	//must be in user mode, disabled
	if (!user_display_mode || !disabled_mode) return;	
	
//	if (mAutoWrite1Btn && mAutoWrite2Btn)
	{
//		pAutonomous.distance = ~mBallLauncherspeed;
//		dirty=1;
//	}
//	if (mAutoWrite1Btn || mAutoWrite2Btn)
//		gUserByteOI = pAutonomous.distance;
//	else if (dirty)
//	{
//		EEPROM_write(kEPROMAdr_Autonomous, pAutonomous.distance);	//save across resets
//		dirty=0;	
//	}
}*/
//********************************************************************************
/*#else	//second method 
void SetAutonomousDistance(void)
{
	static unsigned char keyDownTime=0;
	static char dirty=0;
#define kAutoRepeatTime ((1000/26.2))

	if (dirty &&  ((gLoop.secondz>>1) & 0x1))	//write every other second
	{
		EEPROM_write(kEPROMAdr_Autonomous, pAutonomous.distance);	//save across resets
		dirty=0;
	}
if (gLoop.onSecond)
//	printf("U%d D%d Q%d Up%d Dwn%d  %d\r", (int) user_display_mode,(int) disabled_mode,
//		(int)mBallQueueSw,(int)mAutoTimeUpBtn, (int)mAutoTimeDownBtn, (int)keyDownTime);
		
		
	//must be in user mode, disabled with Meta button pressed to set options
	if (!user_display_mode || !disabled_mode) return;
//	if (!mBallQueueSw) return;

	
	if (mAutoTimeUpBtn)
	{
		if (++keyDownTime==1 || keyDownTime >= kAutoRepeatTime)
			pAutonomous.distance++;
			dirty=1;
	}
	else if (mAutoTimeDownBtn)
	{
		if (++keyDownTime==1 || keyDownTime >= kAutoRepeatTime)
			pAutonomous.distance--;
			dirty=1;
	}
	else
		keyDownTime=0;
	if (keyDownTime==255) keyDownTime=254;	//so's we don't wrap
	
	gUserByteOI = pAutonomous.distance;
}
#endif //end of second method
*/


//******************************************************************************************
//void AutonomousSaveOIControls(void)
//{
//	pAutonomous.dontLaunchBalls = (kOff==threeWaySwitch());
//}
